	ORG     &H000		;Begin program at x000
Init:
	LOAD    Zero		;DISABLE EVERYTHING
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	
	
	
	;OUTPUT to LCD "Toggle Switch 17, press Key1"

	;INSERT A KEY1 Spincall switch
	
MOTOR:
	Load	Zero
	OUT		TIMER
SPIN: 				;spin forward slow
	ILOAD 	SpeedPTR		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	SPIN		;REPEAT FOR 2 SECONDS
	
	IN		LVEL		;Test Left Motor and encoder
	STORE	Speed
	JZERO	LSTOP	;No speed
LSTOPB:	ILOAD	LowPTR
	SUB		Speed
	JPOS	LFAIL	;TOO FAST
	ILOAD	HighPTR	
	SUB		SPEED
	JNEG	LFAIL	;TOO SLOW
LFAILB:
		
	IN		RVEL		;Test Right Motor and encoder
	STORE	Speed
	JZERO	RSTOP	;No speed
RSTOPB:	ILOAD	LowPTR
	SUB		Speed
	JPOS	RFAIL	;TOO FAST
	ILOAD	HighPTR	
	SUB		SPEED
	JNEG	RFAIL	;TOO SLOW
RFAILB:	

ILOAD	SpeedPTR
ADDI	1
ISTORE	SpeedPTR

ILOAD	LowPTR
ADDI	1
ISTORE	LowPTR

ILOAD	HighPTR
ADDI	1
ISTORE	HighPTR
	
LOAD	RepeatSix
ADDI	-1
STORE	RepeatSix
JPOS	MOTOR
	
	
	
;Clear again
	LOAD    Zero
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	LOAD	FAILCODE
	JZERO	PASS
	JUMP	MASTERFAIL
	
	
	
Wait1:
	OUT     TIMER		; One second pause subroutine
Wloop:
	IN      TIMER
	ADDI    -10
	JNEG    Wloop
	RETURN

PASS:
	;PASS STATEMENT, continue with self test
	JUMP	PASS
	
MASTERFAIL:

	JUMP	MASTERFAIL
	
LFAIL:
	;Left Wheel passed threshold
	LOAD	FailCode
	OR		LEFTMOTORVELTHRESH
	STORE	FailCode
	JUMP	LFAILB
	
RFAIL:
	;Right Wheel passed threshold
	LOAD	FailCode
	OR		RIGHTMOTORVELTHRESH
	STORE	FailCode
	JUMP	RFAILB
	
LSTOP:
	;Left Doesn't move
	LOAD	FailCode
	OR		LEFTMOTORNOSTART
	STORE	FailCode
	JUMP	LSTOPB
	
RSTOP:
	;Right doen't move
	LOAD	FailCode
	OR		RIGHTMOTORNOSTART
	STORE	FailCode
	JUMP	RSTOPB
	
	
; This is a good place to put variables
FailCode:			DW 0
LEFTMOTORNOSTART:	DW		4 ; failure of left motor to start
RIGHTMOTORNOSTART:	DW		8 ; failure of left motor to start
LEFTMOTORVELTHRESH:  DW	16 ; failure of left velocity readings to fall within appropriate thresholds
RIGHTMOTORVELTHRESH:  DW	32 ; failure of left velocity readings to fall within appropriate thresholds


Temp:     DW 0
Speed:	  DW 0
RepeatSix: DW 6

SpeedPTR:	DW	Forward1
LowPTR:		DW	RangeF1L
HighPTR:	DW	RangeF1H


Forward1:  DW 70
Forward2:  DW 100
Forward3:  DW 120
Reverse1:  DW -70
Reverse2:  DW -100
Reverse3:  DW -120

RangeF1L:  DW  &H14
RangeF2L:  DW  &H25
RangeF3L:  DW  &H31
RangeR1L:  DW  &HFFE4
RangeR2L:  DW  &HFFD2
RangeR3L:  DW  &HFFC5

RangeF1H:  DW  &H18
RangeF2H:  DW  &H29
RangeF3H:  DW  &H35
RangeR1H:  DW  &HFFE9
RangeR2H:  DW  &HFFD7
RangeR3H:  DW  &HFFCC

; Having some constants can be very useful
Zero:     DW 0
One:      DW 1
Two:      DW 2
Three:    DW 3
Four:     DW 4
Five:     DW 5
Six:      DW 6
Seven:    DW 7
Eight:    DW 8
Nine:     DW 9
Ten:      DW 10
Forward:  DW 70
Reverse:  DW -70
EnSonar0: DW &B00000001
EnSonar1: DW &B00000010
EnSonar2: DW &B00000100
EnSonar3: DW &B00001000
EnSonar4: DW &B00010000
EnSonar5: DW &B00100000
EnSonar6: DW &B01000000
EnSonar7: DW &B10000000
EnSonars: DW &B11111111

; IO address space map
SWITCHES: EQU &H00  ; slide switches
LEDS:     EQU &H01  ; red LEDs
TIMER:    EQU &H02  ; timer, usually running at 10 Hz
XIO:      EQU &H03  ; pushbuttons and some misc. I/0
SEVENSEG: EQU &H04  ; seven-segment display (4-digits only)
LCD:      EQU &H06  ; primitive 4-digit LCD display
LPOS:     EQU &H80  ; left wheel encoder position (read only)
LVEL:     EQU &H82  ; current left wheel velocity (read only)
LVELCMD:  EQU &H83  ; left wheel velocity command (write only)
RPOS:     EQU &H88  ; same values for right wheel...
RVEL:     EQU &H8A  ; ...
RVELCMD:  EQU &H8B  ; ...
I2C_CMD:  EQU &H90  ; I2C module's CMD register,
I2C_DATA: EQU &H91  ; ... DATA register,
I2C_BUSY: EQU &H92  ; ... and BUSY register
SONAR:    EQU &HA0  ; base address for more than 16 registers....
DIST0:    EQU &HA8  ; the eight sonar distance readings
DIST1:    EQU &HA9  ; ...
DIST2:    EQU &HAA  ; ...
DIST3:    EQU &HAB  ; ...
DIST4:    EQU &HAC  ; ...
DIST5:    EQU &HAD  ; ...
DIST6:    EQU &HAE  ; ...
DIST7:    EQU &HAF  ; ...
SONAREN:  EQU &HB2  ; register to control which sonars are enabled
XPOS:     EQU &HC0  ; Current X-position (read only)
YPOS:     EQU &HC1  ; Y-position
THETA:    EQU &HC2  ; Current rotational position of robot (0-701)
